#include "radiation_source_state_machine/state_machine_node.hpp"
#include <rclcpp/rclcpp.hpp>
#include <memory>
#include <chrono>
#include <thread>
#include <algorithm>

namespace radiation_source_state_machine
{

StateMachineNode::StateMachineNode(const rclcpp::NodeOptions& options)
    : Node("radiation_source_state_machine", options),
      current_state_(SystemState::IDLE),
      previous_state_(SystemState::IDLE),
      state_machine_running_(false),
      navigation_reached_(false),
      fixed_movement_complete_(false),
      fine_alignment_complete_(false),
      gripper_complete_(false)
{
    RCLCPP_INFO(this->get_logger(), " createStateMachineNode constructor");
    
     // 从参数服务器获取参数
    params_.source_type = this->get_parameter("source_type").as_string();
    params_.action_type = this->get_parameter("action_type").as_string();
    params_.fixed_movement_distance = this->get_parameter("fixed_movement.distance").as_double();
    params_.fixed_movement_speed = this->get_parameter("fixed_movement.speed").as_double();
    //params_.fixed_movement_duration = this->get_parameter("fixed_movement.duration").as_double();
    params_.max_acceleration = this->get_parameter("max_acceleration").as_double();
    params_.node_activation_timeout = this->get_parameter("timeouts.node_activation").as_double();
    
    // 从YAML获取话题名称
    params_.navigation_reached_topic = this->get_parameter("topics.local_navigation_reached").as_string();
    params_.fixed_movement_complete_topic = this->get_parameter("topics.fixed_movement_complete").as_string();
    params_.fine_alignment_completed_topic = this->get_parameter("topics.fine_alignment_completed").as_string();
    params_.gripper_completed_topic = this->get_parameter("topics.gripper_completed").as_string();
    params_.cmd_vel_topic = this->get_parameter("topics.cmd_vel").as_string();

    RCLCPP_INFO(this->get_logger(), "Parameters loaded from YAML:");
    RCLCPP_INFO(this->get_logger(), "  source_type: %s", params_.source_type.c_str());
    RCLCPP_INFO(this->get_logger(), "  action_type: %s", params_.action_type.c_str());


    // 初始化被管理的节点
    managed_nodes_ = {
        "center_detector",
        "local_navigator", 
        "pointcloud_mqtt_subscriber",
        "center_detector_second",
        "fine_aligner",
        "gripper_communicator"
    };

    // 创建节点客户端和参数客户端
    for (const auto& node_name : managed_nodes_) {
        node_clients_[node_name] = std::make_shared<LifecycleNodeClient>(this->shared_from_this(), node_name);

        // 创建参数客户端（使用完整命名空间）
        param_clients_[node_name] = std::make_shared<rclcpp::SyncParametersClient>(
            this->get_node_base_interface(),
            this->get_node_graph_interface(),
            this->get_node_logging_interface(),
            this->get_node_waitables_interface(),
            "/" + node_name  // 完整命名空间
        );
        RCLCPP_INFO(this->get_logger(), "Created parameter client for: /%s", node_name.c_str());
    }


    // 创建话题发布者和订阅者
    cmd_vel_pub_ = this->create_publisher<std_msgs::msg::String>(params_.cmd_vel_topic, 10);
    fixed_movement_complete_pub_ = this->create_publisher<std_msgs::msg::Bool>(params_.fixed_movement_complete_topic, 10);

    // 创建话题发布者
    navigation_reached_sub_ = this->create_subscription<std_msgs::msg::Bool>(
        params_.navigation_reached_topic, 10,
        std::bind(&StateMachineNode::navigationReachedCallback, this, std::placeholders::_1)
    );
    
    fixed_movement_complete_sub_ = this->create_subscription<std_msgs::msg::Bool>(
        params_.fixed_movement_complete_topic, 10,
        std::bind(&StateMachineNode::fixedMovementCompleteCallback, this, std::placeholders::_1)
    );
    
    fine_alignment_complete_sub_ = this->create_subscription<std_msgs::msg::Bool>(
        params_.fine_alignment_completed_topic, 10,
        std::bind(&StateMachineNode::fineAlignmentCompleteCallback, this, std::placeholders::_1)
    );
    
    gripper_complete_sub_ = this->create_subscription<std_msgs::msg::Bool>(
        params_.gripper_completed_topic, 10,
        std::bind(&StateMachineNode::gripperCompleteCallback, this, std::placeholders::_1)
    );

    // 创建定时器
    state_timer_ = this->create_wall_timer(std::chrono::milliseconds(100),
        std::bind(&StateMachineNode::timerCallback, this)
    );

    RCLCPP_INFO(this->get_logger(), "State machine node initialized successfully");
}


// 配置单个节点参数
bool StateMachineNode::setNodeParameter(const std::string& node_name, const std::string& param_name, const rclcpp::ParameterValue& value)
{
    if (param_clients_.find(node_name) == param_clients_.end()) {
        RCLCPP_ERROR(this->get_logger(), "Parameter client not found for node: %s", node_name.c_str());
        return false;
    }

    auto param_client = param_clients_[node_name];
    
    try {
        // 等待参数服务可用
        if (!param_client->wait_for_service(std::chrono::seconds(2))) {
            RCLCPP_ERROR(this->get_logger(), "Parameters service not available for node: %s", node_name.c_str());
            return false;
        }

        // 设置参数
        std::vector<rclcpp::Parameter> parameters;
        parameters.push_back(rclcpp::Parameter(param_name, value));
        
        auto results = param_client->set_parameters(parameters);
        
        if (results.empty()) {
            RCLCPP_ERROR(this->get_logger(), "Failed to set parameter %s for node %s", param_name.c_str(), node_name.c_str());
            return false;
        }
        
        for (size_t i = 0; i < results.size(); ++i) {
            if (!results[i].successful) {
                RCLCPP_ERROR(this->get_logger(), "Parameter %s set failed for node %s: %s", 
                            param_name.c_str(), node_name.c_str(), results[i].reason.c_str());
                return false;
            }
        }
        
        RCLCPP_INFO(this->get_logger(), "Successfully set parameter %s=%s for node /%s", 
                   param_name.c_str(), value.to_string().c_str(), node_name.c_str());
        return true;
    } catch (const std::exception& e) {
        RCLCPP_ERROR(this->get_logger(), "Exception setting parameter for node %s: %s", node_name.c_str(), e.what());
        return false;
    }
}

// 配置所有节点参数
bool StateMachineNode::configureNodeParameters(const std::string& node_name)
{
    RCLCPP_INFO(this->get_logger(), "Configuring parameters for node: %s", node_name.c_str());
    
    // 为每个节点设置source_type和action_type
    for (const auto& node_name : managed_nodes_) {
        // 设置source_type
        if (!setNodeParameter(node_name, "source_type", rclcpp::ParameterValue(params_.source_type))) {
            RCLCPP_WARN(this->get_logger(), "Failed to set source_type for node %s, continuing...", node_name.c_str());
        }
        
        // 设置action_type
        if (!setNodeParameter(node_name, "action_type", rclcpp::ParameterValue(params_.action_type))) {
            RCLCPP_WARN(this->get_logger(), "Failed to set action_type for node %s, continuing...", node_name.c_str());
        }
        
        // 特定节点的特殊参数！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！
        if (node_name == "center_detection") {
            // 根据source_type设置检测模式
            std::string detection_mode = params_.source_type; // neutron or gamma
            if (!setNodeParameter(node_name, "detection_mode", rclcpp::ParameterValue(detection_mode))) {
                RCLCPP_WARN(this->get_logger(), "Failed to set detection_mode for center_detection");
            }
        }
    }
    
    RCLCPP_INFO(this->get_logger(), "All node parameters configured successfully");
    return true;
}

StateMachineNode::~StateMachineNode()
{
    stopStateMachine();
    if (state_machine_thread_.joinable()) {
        state_machine_thread_.join();
    }
}

void StateMachineNode::timerCallback()
{
    runStateMachine();
}

void StateMachineNode::startStateMachine()
{
    std::lock_guard<std::mutex> lock(state_mutex_);
    state_machine_running_ = true;
    current_state_ = SystemState::IDLE;
    previous_state_ = SystemState::IDLE;
    state_cv_.notify_one();
    RCLCPP_INFO(this->get_logger(), "State machine started");
}

void StateMachineNode::stopStateMachine()
{
    std::lock_guard<std::mutex> lock(state_mutex_);
    state_machine_running_ = false;
    state_cv_.notify_one();
    RCLCPP_INFO(this->get_logger(), "State machine stopped");
}

void StateMachineNode::runStateMachine()
{
    if (!state_machine_running_) return;

    switch (static_cast<SystemState>(current_state_.load())) {
        case SystemState::IDLE:
            handleIdleState();
            break;
        case SystemState::SOURCE_DETECTION_FIRST:
            handleSourceDetectionFirstState();
            break;
        case SystemState::LOCAL_NAVIGATION:
            handleLocalNavigationState();
            break;
        case SystemState::FIXED_MOVEMENT:
            handleFixedMovementState();
            break;
        case SystemState::MQTT_CLOUD_SUB_START:
            handleMqttCloudSubStartState();
            break;
        case SystemState::SOURCE_DETECTION_SECOND:
            handleSourceDetectionSecondState();
            break;
        case SystemState::FINE_ALIGNMENT:
            handleFineAlignmentState();
            break;
        case SystemState::GRIPPER_CONTROL:
            handleGripperControlState();
            break;
        case SystemState::COMPLETED:
            handleCompletedState();
            break;
        case SystemState::ERROR:
            handleErrorState();
            break;
        default:
            RCLCPP_WARN(this->get_logger(), "Unknown state: %d", static_cast<int>(current_state_.load()));
            transitionToState(SystemState::ERROR);
            break;
    }
}

void StateMachineNode::transitionToState(SystemState new_state)
{
    SystemState old_state = current_state_.load();
    previous_state_.store(old_state);
    current_state_.store(new_state);
    
    RCLCPP_INFO(this->get_logger(), "State transition: %d -> %d", 
               static_cast<int>(old_state), static_cast<int>(new_state));
    
    cleanupStateResources();
}

void StateMachineNode::handleIdleState()
{
    RCLCPP_INFO(this->get_logger(), "State: IDLE");

    // 配置所有节点参数
    if (!configureNodeParameters()) {
        RCLCPP_ERROR(this->get_logger(), "Failed to configure node parameters");
        transitionToState(SystemState::ERROR);
        return;
    }
    
    publishTransientTopics();
    
    std::this_thread::sleep_for(std::chrono::milliseconds(500));
    
    transitionToState(SystemState::SOURCE_DETECTION_FIRST);
}

void StateMachineNode::handleSourceDetectionFirstState()
{
    RCLCPP_INFO(this->get_logger(), "State: SOURCE_DETECTION_FIRST");
    
    if (startNode("center_detection")) {
        if (node_clients_["center_detection"]->wait_for_state(
            lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, std::chrono::seconds(5))) {
            
            RCLCPP_INFO(this->get_logger(), "Center detection node is active");
            
            std::this_thread::sleep_for(std::chrono::seconds(3));
            
            transitionToState(SystemState::LOCAL_NAVIGATION);
        } else {
            RCLCPP_ERROR(this->get_logger(), "Center detection node failed to activate");
            transitionToState(SystemState::ERROR);
        }
    } else {
        RCLCPP_ERROR(this->get_logger(), "Failed to start center detection node");
        transitionToState(SystemState::ERROR);
    }
}

void StateMachineNode::handleLocalNavigationState()
{
    RCLCPP_INFO(this->get_logger(), "State: LOCAL_NAVIGATION");
    
    if (startNode("local_navigation")) {
        if (node_clients_["local_navigation"]->wait_for_state(
            lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, std::chrono::seconds(5))) {
            
            RCLCPP_INFO(this->get_logger(), "Local navigation node is active");
            
            if (navigation_reached_.load()) {
                RCLCPP_INFO(this->get_logger(), "Navigation reached target");
                navigation_reached_.store(false);

                // 统一关闭 center_detection 和 local_navigation 节点
                RCLCPP_INFO(this->get_logger(), "Navigation reached, now closing center_detection and local_navigation!!");

                // 先关闭 center_detection 节点
                safeStopNode("center_detection");

                // 再关闭 local_navigation 节点
                safeStopNode("local_navigation");

                RCLCPP_INFO(this->get_logger(), "BOTH NODES CLOSED SUCCESSFULLY! PROCEEDING TO FIXED MOVEMENT");

                transitionToState(SystemState::FIXED_MOVEMENT);
            }
        } else {
            RCLCPP_ERROR(this->get_logger(), "Local navigation node failed to activate");
            // 激活失败时也要尝试关闭节点
            RCLCPP_WARN(this->get_logger(), "NAVIGATION FAILED - ATTEMPTING TO CLEAN UP NODES");
            safeStopNode("center_detection");
            safeStopNode("local_navigation");

            transitionToState(SystemState::ERROR);
        }
    } else {
        RCLCPP_ERROR(this->get_logger(), "Failed to start local navigation node");
        // 启动失败时也要尝试关闭 center_detection
        RCLCPP_WARN(this->get_logger(), "FAILED TO START NAVIGATION - CLEANING UP center_detection");
        safeStopNode("center_detection");

        transitionToState(SystemState::ERROR);
    }
}

void StateMachineNode::handleFixedMovementState()
{
    RCLCPP_INFO(this->get_logger(), "State: FIXED_MOVEMENT");
    
    publishTwistCommand(fixed_movement_speed_, 0.0, fixed_movement_duration_);
    
    if (fixed_movement_complete_.load()) {
        RCLCPP_INFO(this->get_logger(), "Fixed movement completed");
        fixed_movement_complete_.store(false);
        transitionToState(SystemState::MQTT_CLOUD_SUB_START);
    }
}

void StateMachineNode::handleMqttCloudSubStartState()
{
    RCLCPP_INFO(this->get_logger(), "State: MQTT_CLOUD_SUB_START");
    
    if (startNode("mqtt_cloud_sub")) {
        if (node_clients_["mqtt_cloud_sub"]->wait_for_state(
            lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, std::chrono::seconds(5))) {
            
            RCLCPP_INFO(this->get_logger(), "MQTT cloud subscriber is active");
            
            transitionToState(SystemState::SOURCE_DETECTION_SECOND);
        } else {
            RCLCPP_ERROR(this->get_logger(), "MQTT cloud subscriber failed to activate");
            transitionToState(SystemState::ERROR);
        }
    } else {
        RCLCPP_ERROR(this->get_logger(), "Failed to start MQTT cloud subscriber");
        transitionToState(SystemState::ERROR);
    }
}

void StateMachineNode::handleSourceDetectionSecondState()
{
    RCLCPP_INFO(this->get_logger(), "State: SOURCE_DETECTION_SECOND");
    
    if (startNode("center_detection_second")) {
        if (node_clients_["center_detection_second"]->wait_for_state(
            lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, std::chrono::seconds(5))) {
            
            RCLCPP_INFO(this->get_logger(), "Center detection second node (second) is active");
            
            std::this_thread::sleep_for(std::chrono::seconds(3));
            
            transitionToState(SystemState::FINE_ALIGNMENT);
        } else {
            RCLCPP_ERROR(this->get_logger(), "Center detection second node (second) failed to activate");
            transitionToState(SystemState::ERROR);
        }
    } else {
        RCLCPP_ERROR(this->get_logger(), "Failed to start center detection second node (second)");
        transitionToState(SystemState::ERROR);
    }
}

void StateMachineNode::handleFineAlignmentState()
{
    RCLCPP_INFO(this->get_logger(), "State: FINE_ALIGNMENT");
    
    if (startNode("fine_alignment")) {
        if (node_clients_["fine_alignment"]->wait_for_state(
            lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, std::chrono::seconds(5))) {
            
            RCLCPP_INFO(this->get_logger(), "Fine alignment node is active");
            
            if (fine_alignment_complete_.load()) {
                RCLCPP_INFO(this->get_logger(), "Fine alignment completed");
                fine_alignment_complete_.store(false);

                // 统一关闭 center_detection 和 fine_alignment 节点
                RCLCPP_INFO(this->get_logger(), "Navigation reached, now closing center_detection and fine_alignment!!");

                // 先关闭 center_detection 节点
                safeStopNode("center_detection");

                // 再关闭 fine_alignment 节点
                safeStopNode("fine_alignment");

                RCLCPP_INFO(this->get_logger(), "BOTH NODES CLOSED SUCCESSFULLY! PROCEEDING TO GRIPPER_CONTROL");

                transitionToState(SystemState::GRIPPER_CONTROL);
            }
        } else {
            RCLCPP_ERROR(this->get_logger(), "Fine alignment node failed to activate");
            // 激活失败时也要尝试关闭节点
            RCLCPP_WARN(this->get_logger(), "ALIGNMENT FAILED - ATTEMPTING TO CLEAN UP NODES");
            safeStopNode("center_detection");
            safeStopNode("fine_alignment");

            transitionToState(SystemState::ERROR);
        }
    } else {
        RCLCPP_ERROR(this->get_logger(), "Failed to start fine alignment node");
        // 启动失败时也要尝试关闭 center_detection
        RCLCPP_WARN(this->get_logger(), "FAILED TO START NAVIGATION - CLEANING UP center_detection");
        safeStopNode("center_detection");

        transitionToState(SystemState::ERROR);
    }
}

void StateMachineNode::handleGripperControlState()
{
    RCLCPP_INFO(this->get_logger(), "State: GRIPPER_CONTROL");
    
    if (startNode("gripper_control")) {
        if (node_clients_["gripper_control"]->wait_for_state(
            lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, std::chrono::seconds(5))) {
            
            RCLCPP_INFO(this->get_logger(), "Gripper control node is active");
            
            if (gripper_complete_.load()) {
                RCLCPP_INFO(this->get_logger(), "Gripper operation completed");
                gripper_complete_.store(false);
                transitionToState(SystemState::COMPLETED);
            }
        } else {
            RCLCPP_ERROR(this->get_logger(), "Gripper control node failed to activate");
            transitionToState(SystemState::ERROR);
        }
    } else {
        RCLCPP_ERROR(this->get_logger(), "Failed to start gripper control node");
        transitionToState(SystemState::ERROR);
    }
}

void StateMachineNode::handleCompletedState()
{
    RCLCPP_INFO(this->get_logger(), "State: COMPLETED");

    // 确保所有节点都被正确关闭
    RCLCPP_INFO(this->get_logger(), "FINAL CLEANUP: ENSURING ALL NODES ARE PROPERLY STOPPED");
    for (const auto& node_name : managed_nodes_) {
        RCLCPP_INFO(this->get_logger(), "Final cleanup for node: %s", node_name.c_str());
        safeStopNode(node_name);
    }
    
    RCLCPP_INFO(this->get_logger(), "All nodes stopped. Task completed successfully!");
}

void StateMachineNode::handleErrorState()
{
    RCLCPP_ERROR(this->get_logger(), "State: ERROR");
    
    RCLCPP_INFO(this->get_logger(), "CLEANUP FOR ERROR: ENSURING ALL NODES ARE PROPERLY STOPPED");
    for (const auto& node_name : managed_nodes_) {
        RCLCPP_INFO(this->get_logger(), " error cleanup for node: %s", node_name.c_str());
        safeStopNode(node_name);
    }
    
    RCLCPP_ERROR(this->get_logger(), "State machine entered error state. Manual intervention required.");
}

bool StateMachineNode::startNode(const std::string& node_name)
{
    RCLCPP_INFO(this->get_logger(), "Starting node: %s", node_name.c_str());

    // 配置节点参数
    if (!configureNodeParameters()) {
        RCLCPP_ERROR(this->get_logger(), "Failed to configure node parameters for: %s", node_name.c_str());
        return false;
    }

    if (node_clients_.find(node_name) == node_clients_.end()) {
        RCLCPP_ERROR(this->get_logger(), "Node client not found for: %s", node_name.c_str());
        return false;
    }

    auto& node_client = node_clients_[node_name];
    
    RCLCPP_INFO(this->get_logger(), "Starting node: %s", node_name.c_str());
    
    if (!node_client->configure()) {
        RCLCPP_ERROR(this->get_logger(), "Failed to configure node: %s", node_name.c_str());
        return false;
    }
    
    if (!node_client->activate()) {
        RCLCPP_ERROR(this->get_logger(), "Failed to activate node: %s", node_name.c_str());
        return false;
    }
    
    RCLCPP_INFO(this->get_logger(), "Node %s started successfully", node_name.c_str());
    return true;
}

bool StateMachineNode::safeStopNode(const std::string& node_name)
{
    if (node_clients_.find(node_name) == node_clients_.end()) {
        RCLCPP_WARN(this->get_logger(), " Node client not found for: %s", node_name.c_str());
        return false;
    }

    auto& node_client = node_clients_[node_name];
    
    try {
        RCLCPP_INFO(this->get_logger(), "Safely stopping node: %s", node_name.c_str());
        
        // 首先检查当前状态
        auto current_state = node_client->get_current_state();
        RCLCPP_INFO(this->get_logger(), "Current state of %s: %s (ID: %d)", 
                   node_name.c_str(), current_state.label.c_str(), current_state.id);
        
        // 只在活动状态或非活跃状态下才需要停止
        if (current_state.id == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) {
            RCLCPP_INFO(this->get_logger(), "Node is ACTIVE - deactivating...");
            if (!node_client->deactivate()) {
                RCLCPP_WARN(this->get_logger(), "Failed to deactivate node: %s", node_name.c_str());
            }
        }
        
        // 清理资源
        RCLCPP_INFO(this->get_logger(), "Cleaning up node resources: %s", node_name.c_str());
        if (!node_client->cleanup()) {
            RCLCPP_WARN(this->get_logger(), "Failed to cleanup node: %s", node_name.c_str());
        }
        
        RCLCPP_INFO(this->get_logger(), "Node %s stopped safely", node_name.c_str());
        return true;
        
    } catch (const std::exception& e) {
        RCLCPP_ERROR(this->get_logger(), "Exception while stopping node %s: %s", 
                    node_name.c_str(), e.what());
        return false;
    }
}

// 速度平滑函数
void LocalNavigator::applyVelocitySmoothing(double& linear_x, double& linear_y, double& angular_z,
                                        double target_linear_x, double target_linear_y, double target_angular_z) {
    
    double dt = static_cast<double>(controlling_period_) / 1000.0;
    double max_linear_change = max_acceleration_ * dt;
    double max_angular_change = max_angular_acceleration_ * dt;
    
    double dx_change = std::clamp(target_linear_x - prev_linear_x_, 
                                -max_linear_change, max_linear_change);
    double dy_change = std::clamp(target_linear_y - prev_linear_y_, 
                                -max_linear_change, max_linear_change);
    double dtheta_change = std::clamp(target_angular_z - prev_angular_z_, 
                                    -max_angular_change, max_angular_change);
                                    
    linear_x = prev_linear_x_ + dx_change;
    linear_y = prev_linear_y_ + dy_change;
    angular_z = prev_angular_z_ + dtheta_change;

    prev_linear_x_ = linear_x;
    prev_linear_y_ = linear_y;
    prev_angular_z_ = angular_z;

    RCLCPP_INFO(this->get_logger(), "dt=%.3f, max_linear_change=%.3f, dx_change=%.3f, dy_change=%.3f", 
                dt, max_linear_change, dx_change, dy_change);
    RCLCPP_INFO(this->get_logger(), "prev_linear_x_=%.3f, prev_linear_y_=%.3f, prev_angular_z_=%.3f", 
                prev_linear_x_, prev_linear_y_, prev_angular_z_);
}

// 运动时间计算函数
double LocalNavigator::calculateMovementTime(double distance, double max_vel, double max_acc)
{
    if (distance < 1e-6) return 0.0;

    // 计算加速到最大速度所需的时间和距离
    double time_to_max_vel = max_vel / max_acc;
    double distance_to_max_vel = 0.5 * max_acc * time_to_max_vel * time_to_max_vel;
    
    //RCLCPP_INFO(this->get_logger(),"最大加速时间：%.3f, 最大加速距离：%.3f",time_to_max_vel, distance_to_max_vel);

    // 如果距离足够让机器人达到最大速度（梯形模式）
    if (distance >= 2.0 * distance_to_max_vel) {
        // 梯形模式：加速 -> 匀速 -> 减速
        double constant_vel_distance = distance - 2.0 * distance_to_max_vel;
        double constant_vel_time = constant_vel_distance / max_vel;

        RCLCPP_INFO(this->get_logger(),"实际移动时间：%.3f", 2.0 * time_to_max_vel + constant_vel_time);
        
        return 2.0 * time_to_max_vel + constant_vel_time;

    } else {
        // 三角形模式：加速 -> 减速（达不到最大速度）
        // distance = 0.5 * max_acc * t_acc^2 + 0.5 * max_acc * t_dec^2
        // 由于对称，t_acc = t_dec，所以 distance = max_acc * t_acc^2
        double accel_time = std::sqrt(distance / max_acc);

        RCLCPP_INFO(this->get_logger(),"实际移动时间：%.3f",2.0 * accel_time);
        
        return 2.0 * accel_time;

    }
}

void StateMachineNode::publishTwistCommand(double linear_x, double angular_z, double duration_sec)
{
    RCLCPP_INFO(this->get_logger(), "Publishing twist command: linear=%.2f, angular=%.2f, duration=%.1fs", 
               linear_x, angular_z, duration_sec);
    
    auto twist_msg = std::make_unique<geometry_msgs::msg::Twist>();
    twist_msg->linear.x = linear_x;
    twist_msg->angular.z = angular_z;
    
    // Start timer to stop movement after duration
    fixed_movement_timer_ = this->create_wall_timer(
        std::chrono::milliseconds(static_cast<int>(duration_sec * 1000)),
        [this]() {
            RCLCPP_INFO(this->get_logger(), "Fixed movement duration completed");
            
            auto stop_msg = std::make_unique<geometry_msgs::msg::Twist>();
            stop_msg->linear.x = 0.0;
            stop_msg->angular.z = 0.0;
            cmd_vel_pub_->publish(std::move(stop_msg));
            
            auto complete_msg = std::make_unique<std_msgs::msg::Bool>();
            complete_msg->data = true;
            fixed_movement_complete_pub_->publish(std::move(complete_msg));
            
            if (fixed_movement_timer_) {
                fixed_movement_timer_->cancel();
            }
        }
    );
    
    cmd_vel_pub_->publish(std::move(twist_msg));
}

void StateMachineNode::publishTransientTopics()
{
    RCLCPP_INFO(this->get_logger(), "Publishing transient topics: source_type=%s, action_type=%s",
               source_type_.c_str(), action_type_.c_str());
    
    auto source_msg = std::make_unique<std_msgs::msg::String>();
    source_msg->data = source_type_;
    source_type_pub_->publish(std::move(source_msg));
    
    auto action_msg = std::make_unique<std_msgs::msg::String>();
    action_msg->data = action_type_;
    action_type_pub_->publish(std::move(action_msg));
    
    RCLCPP_INFO(this->get_logger(), "Transient topics published successfully");
}

void StateMachineNode::cleanupStateResources()
{
    RCLCPP_DEBUG(this->get_logger(), "Cleaning up resources for previous state");
    
    if (fixed_movement_timer_) {
        fixed_movement_timer_->cancel();
        fixed_movement_timer_.reset();
    }
    
    navigation_reached_.store(false);
    fixed_movement_complete_.store(false);
    fine_alignment_complete_.store(false);
    gripper_complete_.store(false);
}

void StateMachineNode::sourceTypeCallback(const std_msgs::msg::String::SharedPtr msg)
{
    std::lock_guard<std::mutex> lock(state_mutex_);
    source_type_ = msg->data;
    RCLCPP_DEBUG(this->get_logger(), "Received source_type: %s", source_type_.c_str());
}

void StateMachineNode::actionTypeCallback(const std_msgs::msg::String::SharedPtr msg)
{
    std::lock_guard<std::mutex> lock(state_mutex_);
    action_type_ = msg->data;
    RCLCPP_DEBUG(this->get_logger(), "Received action_type: %s", action_type_.c_str());
}

void StateMachineNode::navigationReachedCallback(const std_msgs::msg::Bool::SharedPtr msg)
{
    navigation_reached_.store(msg->data);
    RCLCPP_INFO(this->get_logger(), "Navigation reached callback: %s", 
               navigation_reached_.load() ? "true" : "false");
}

void StateMachineNode::fixedMovementCompleteCallback(const std_msgs::msg::Bool::SharedPtr msg)
{
    fixed_movement_complete_.store(msg->data);
    RCLCPP_INFO(this->get_logger(), "Fixed movement complete callback: %s", 
               fixed_movement_complete_.load() ? "true" : "false");
}

void StateMachineNode::fineAlignmentCompleteCallback(const std_msgs::msg::Bool::SharedPtr msg)
{
    fine_alignment_complete_.store(msg->data);
    RCLCPP_INFO(this->get_logger(), "Fine alignment complete callback: %s", 
               fine_alignment_complete_.load() ? "true" : "false");
}

void StateMachineNode::gripperCompleteCallback(const std_msgs::msg::Bool::SharedPtr msg)
{
    gripper_complete_.store(msg->data);
    RCLCPP_INFO(this->get_logger(), "Gripper complete callback: %s", 
               gripper_complete_.load() ? "true" : "false");
}

}  // namespace radiation_source_state_machine
